/*
 *  libCV.c
 *  libCV
 *
 *  Created by Gregory Koch on 14/01/11.
 *  Copyright 2011 IFlyBotIV. All rights reserved.
 *
 */

#include "libCV.h"
#include "histogram.h"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <math.h>

cvPos_t init_pos(int x, int y)
{
	cvPos_t pos;
	
	pos.x = x;
	pos.y = y;
	
	return pos;
}

cvSize_t init_size(int width, int height)
{
	cvSize_t size;
	
	size.height = height;
	size.width = width;
	
	return size;
}

image_t init_image(int type, cvSize_t size)
{
	image_t image;
	
	image.type = type;
	image.size.height = size.height;
	image.size.width = size.width;
	
	if(type == IMG_BGR24 || type == IMG_RGB24)
	{
		image.stride = size.width * 3;
		
		image.data = malloc(sizeof(char)*size.height*size.width*3);
		if(image.data == NULL)
		{
			perror("libCV : malloc error");
			exit(EXIT_FAILURE);
		}
	}
	else if(type == IMG_BINARY8 || type == IMG_GREYSCALE8)
	{
		image.stride = size.width;
		
		image.data = malloc(sizeof(char)*size.height*size.width);
		if(image.data == NULL)
		{
			perror("libCV : malloc error");
			exit(EXIT_FAILURE);
		}
	}
	else
	{
		perror("libCV : Unknow image type");
		exit(EXIT_FAILURE);
	}
    
	return image;
}

image_t set_image(int type, cvSize_t size, int stride, unsigned char* datas)
{
	image_t image;
	
	image.type = type;
	image.size.height = size.height;
	image.size.width = size.width;
	
	image.stride = stride;
    
	image.data = datas;
    
	return image;	
}

void free_image(image_t image)
{
	if(image.data)
	{
		free(image.data);
	}	
}

zone_t init_zone(cvPos_t pos, cvSize_t size)
{
	zone_t zone;
	
	zone.pos.x = pos.x;
	zone.pos.y = pos.y;
	zone.size.height = size.height;
	zone.size.width = size.width;
	
	return zone;
}

int verif_zone(image_t img, zone_t zone)
{	
	if(zone.pos.x >= 0 && (img.size.width - (zone.pos.x + zone.size.width)) >= 0)
	{
		if(zone.pos.y >= 0 && (img.size.height - (zone.pos.y + zone.size.height)) >= 0)
		{
			return 1;
		}
	}
	
	return 0;
}

void move_zone(zone_t *zone, cvPos_t pos, image_t img)
{
	zone_t tmp = init_zone(init_pos(pos.x, pos.y),
						   init_size(zone->size.width, zone->size.height));
	
	
	if(verif_zone(img, tmp))
	{
		*zone = tmp;
	}
	else
	{
		if(tmp.pos.x < 0)
		{
			tmp.pos.x = 0;
		}
		if(tmp.pos.x + tmp.size.width > img.size.width)
		{
			tmp.pos.x = img.size.width - tmp.size.width;
		}
		
		if(tmp.pos.y < 0)
		{
			tmp.pos.y = 0;
		}
		if(tmp.pos.y + tmp.size.height > img.size.height)
		{
			tmp.pos.y = img.size.height - tmp.size.height;
		}
		
		if(verif_zone(img, tmp))
		{
			*zone = tmp;
		}
		else
		{
			fprintf(stderr, "Error trying to move a zone out of the image\n");
		}
        
	}	
}

void resize_zone(zone_t *zone, cvSize_t size, image_t img)
{
	zone_t tmp;
	
	tmp = init_zone( init_pos(zone->pos.x + (zone->size.width - size.width)/2, 
                              zone->pos.y + (zone->size.height - size.height)/2), 
                    init_size(size.width, size.height));
	
	
	if(verif_zone(img, tmp))
	{
		*zone = tmp;
	}
	else
	{
		if(tmp.pos.x + tmp.size.width > img.size.width)
		{
			tmp.size.width -= tmp.pos.x + tmp.size.width - img.size.width;
		}
		
		if(tmp.pos.y + tmp.size.height > img.size.height)
		{
			tmp.size.height -= tmp.pos.y + tmp.size.height - img.size.height;
		}
		
		if(tmp.pos.x < 0)
		{
			tmp.size.width += tmp.pos.x; 
			tmp.pos.x = 0;
		}
		
		if(tmp.pos.y < 0)
		{
			tmp.size.height += tmp.pos.y; 
			tmp.pos.y = 0;
		}
		
		if(verif_zone(img, tmp))
		{
			*zone = tmp;
		}
		else
		{
			fprintf(stderr, "Error trying to resize a zone\n");
		}
	}
}

void center_zone(zone_t *zone, cvPos_t pos, image_t img)
{
	zone_t tmp;
	
	tmp = init_zone(init_pos(zone->pos.x + (pos.x - zone->size.width/2), 
                             zone->pos.y + (pos.y - zone->size.height/2)), 
                    init_size(zone->size.width, zone->size.height));
	
	
	if(verif_zone(img, tmp))
	{
		*zone = tmp;
	}
	else
	{
		if(tmp.pos.x < 0)
		{
			tmp.size.width += tmp.pos.x;
			tmp.pos.x = 0;
		}
		if(tmp.pos.x + tmp.size.width > img.size.width)
		{
			tmp.size.width -= tmp.pos.x + tmp.size.width - img.size.width;
		}
		
		if(tmp.pos.y < 0)
		{
			tmp.size.height += tmp.pos.y;
			tmp.pos.y = 0;
		}
		if(tmp.pos.y + tmp.size.height > img.size.height)
		{
			tmp.size.height -= tmp.pos.y + tmp.size.height - img.size.height;
		}
		
		if(verif_zone(img, tmp))
		{
			*zone = tmp;
		}
		else
		{
			fprintf(stderr, "Error trying to center a zone\n");
		}
	}
}

void draw_zone(image_t img, zone_t zone)
{
	int x, y, pos1, pos2;
	
	if(!verif_zone(img, zone))
	{
		fprintf(stderr, "libCV : zone out of image\n");
		exit(EXIT_FAILURE);
	}
	
	if(img.type == IMG_BGR24 || img.type == IMG_RGB24)
	{
		for(x=0; x<zone.size.width; x++)
		{
			pos1 = img.stride * (zone.pos.y)
			+ (zone.pos.x + x) * 3;
			
			pos2 = img.stride * (zone.pos.y + zone.size.height-1)
			+ (zone.pos.x + x) * 3;
			
			img.data[pos1] = 255;
			img.data[pos2] = 255;
			img.data[pos1+1] = 255;
			img.data[pos2+1] = 255;
			img.data[pos1+2] = 255;
			img.data[pos2+2] = 255;
		}	
		
		for(y=0; y<zone.size.height; y++)
		{
			pos1 = img.stride * (zone.pos.y + y)
			+ (zone.pos.x) * 3;
			
			pos2 = img.stride * (zone.pos.y + y)
			+ (zone.pos.x + zone.size.width-1) * 3;
			
			img.data[pos1] = 255;
			img.data[pos2] = 255;
			img.data[pos1+1] = 255;
			img.data[pos2+1] = 255;
			img.data[pos1+2] = 255;
			img.data[pos2+2] = 255;
		}		
	}
	else if(img.type == IMG_GREYSCALE8)
	{
		for(x=0; x<zone.size.width; x++)
		{
			pos1 = img.stride * (zone.pos.y)
			+ (zone.pos.x + x);
			
			pos2 = img.stride * (zone.pos.y + zone.size.height-1)
			+ (zone.pos.x + x);
			
			img.data[pos1] = 255;
			img.data[pos2] = 255;
		}	
		
		for(y=0; y<zone.size.height; y++)
		{
			pos1 = img.stride * (zone.pos.y + y)
			+ (zone.pos.x);
			
			pos2 = img.stride * (zone.pos.y + y)
			+ (zone.pos.x + zone.size.width-1);
			
			img.data[pos1] = 255;
			img.data[pos2] = 255;
		}		
	}
	else if(img.type == IMG_BINARY8)
	{
		for(x=0; x<zone.size.width; x++)
		{
			pos1 = img.stride * (zone.pos.y)
			+ (zone.pos.x + x);
			
			pos2 = img.stride * (zone.pos.y + zone.size.height-1)
			+ (zone.pos.x + x);
			
			img.data[pos1] = 1;
			img.data[pos2] = 1;
		}	
		
		for(y=0; y<zone.size.height; y++)
		{
			pos1 = img.stride * (zone.pos.y + y)
			+ (zone.pos.x);
			
			pos2 = img.stride * (zone.pos.y + y)
			+ (zone.pos.x + zone.size.width-1);
			
			img.data[pos1] = 1;
			img.data[pos2] = 1;
		}		
	}
	else
	{
		perror("libCV : Unknow image type");
		exit(EXIT_FAILURE);
	}
}

float dist(cvPos_t pos1, cvPos_t pos2)
{
	return sqrt(pow(pos1.x-pos2.x, 2) + pow(pos1.y-pos2.y, 2));
}

float kernel_dist(zone_t zone, cvPos_t pos)
{
	float dist, dist_max;
	
	dist_max = sqrt(pow(-1-zone.size.width/2, 2) + pow(-1-zone.size.height/2, 2));
	
	dist = sqrt(pow(pos.x-zone.size.width/2, 2) + pow(pos.y-zone.size.height/2, 2));
	
	return 1 - dist/dist_max;
}
